/*
 * FlightController.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#include "common.h"
#include "FlightController_Common.h"
#include "IMU.h"
#include "main.h"
#include "Serial.h"
#include "Debug.h"
#include "RemoteControl.h"
#include "MotorControl.h"
#include "EEPROM.h"
#include "CommProtocol.h"
#include "Alarms.h"
#include "GPS_common.h"

/******************************************************************************/
/* Constant Definition                                                        */
/******************************************************************************/


/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/
uint32_t currentLoopTimeStamp = 0;  /* Time stamp that is updated every loop */
uint16_t previousLoopTimeStamp = 0; /* Time stamp for previous loop. It is
                                       reduced to uint16 for RAM savings purposes */
uint16_t loopCycleTime = 0;         /* Time in microseconds that the last loop execution took */

uint8_t calibrationDataUpdateRequired = 0;

imu_t imuData;                      /* IMU sensors data for each axis */
alt_t baroData;                     /* Barometric data structure for altitude and ascend/descend speed */
analog_t analogData;
alt_t alt;
att_t att;

cal_t calibration;                  /* structure that holds initial calibration cycles for the IMU Sensors */
calibration_data_t calData;         /* structure that holds calibration data for sensors and is tored in the EEPROM */

flags_struct_t globalFlags;         /* Define global flags structure used by different APIs */

int16_t rcData[RC_CHANNELS_NUM];    /* interval [1000;2000] */
int16_t rcCommand[4];               /* this will hold command for ROLL + PITCH + YAW + THROTTLE controls */
int16_t motorControlData[8];        /* This array holds the commands for motor PWMs wo be written */
int16_t servoControlData[8] = {1500,1500,1500,1500,1500,1500,1500,1000};
int16_t axisPID[3];

/* PID Settings */
uint8_t dynP8[2];
uint8_t dynD8[2];

int32_t AltHold;                    /* in cm */
int16_t magHold;
int16_t headFreeModeHold;           /* [-180;+180] */
int16_t BaroPID = 0;
int16_t errorAltitudeI = 0;
int16_t initialThrottleHold;

#if defined(THROTTLE_ANGLE_CORRECTION)
int16_t throttleAngleCorrection = 0;  /* correction of throttle in lateral wind, */
int8_t  cosZ = 100;                   /* cos(angleZ)*100 */
#endif /* defined(THROTTLE_ANGLE_CORRECTION) */

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
void testFlow(void);
static void FlightController_ModeTweaks(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: FlightController_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_Init(void)
{
    /* Read config parameters from EEPROM */
    if (EEPROM_ReadCalibrationData() == false)
    {
        /* The calibration data is broken, initialize to defaults */
        FC_LoadCalibrationDefaults();

        /* Set the flag to let the system know that the data should be updated in EEPROM when calibration is done */
        calibrationDataUpdateRequired = 1;
    }

    /* Load Configuration and Calibration Data from EEPROM.
     * If the integrity check fails - force the default values
     * and write them back to EEPROM */
    FlightController_LoadConfiguration();

    /* Initialize IMU board */
    IMU_Init();

#if GPS_INSTALLED
    /* Read GPS Configuration from EEPROM */
    EEPROM_ReadGPSConfig();

    /* Initialize GPS */
    GPS_Init();
#endif /* GPS_INSTALLED */

    /* Initialize Receiver */
    RC_ConfigureReceiver();

    /* Initialize Alarms */
    Alarms_Init();

    /* Initialize Motor Outputs */
    MotorControl_Init();
}

/*******************************************************************************
* Function Name: FlightController_Main()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_Main(void)
{
    /* Run firmware main loop */
    while(1)
    {
        FlightController_Process(micros());
    }
}

/*******************************************************************************
* Function Name: FlightController_Process()
********************************************************************************
* Summary:
*  Main processing loop executes inside this function
*
* Parameters:
*  uint32_t currentTimeStamp - current time stamp of the microsecond timer
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_Process(uint32_t currentTimeStamp)
{
    static uint16_t rcCheckTime = 0;


    /* Check if it's time to make RC readings */
    if ((int16_t)(currentLoopTimeStamp - rcCheckTime) > 0)
    {
        /* Add 20ms to the time stamp to achieve 50Hz rate (can be changed in FlightController.h) */
        rcCheckTime = currentLoopTimeStamp + RC_CHECK_TIME_PERIOD_IN_MICROS;

        /* Read and process data from the receiver.
         * The data will be stored in the global array rcData[] */
        RC_ProcessReceivedData();

        /* Process Stick commands */
        FlightController_StickCommandHandler();

        /* Increment Failsafe counter every time we are in the check RC loop.
         * It will be cleared in the interrupt if all channels recevied correctly */
#if defined(FAILSAFE)
        if (Failsafe_IsRCFailsafeActive())
        {
            Failsafe_EnterRCFailsafeState();
        }

        Failsafe_IncrementRCCounter();
#endif /* defined(FAILSAFE) */
    }
    else /* Not a time to read RC data -> perform other functions */
    {
        /* Process IMU Sensor readings (Accelerometer + Gyroscope) */
        IMU_Process();

        /* Save current loop and cycle time in this place */
        currentLoopTimeStamp = currentTimeStamp;
        loopCycleTime = currentLoopTimeStamp - previousLoopTimeStamp;
        previousLoopTimeStamp = currentLoopTimeStamp;

        //testFlow();

        /* Process Communication Routine */
        CommProtocol_Process();

        /* Apply some controls corrections in special modes */
        FlightController_ModeTweaks();

        /* Calculate Angle Correction based on GPS Data */
#if GPS_INSTALLED
        GPS_CalculateAngles();
#endif /* GPS_INSTALLED */

        /* Calculate PID for PITCH, ROLL and YAW */
        FlihgtController_CalculatePID();

        /* Mix the table for Motor Outputs */
        MotorControl_MixTable();

        /* Write Motor Values Here */
        MotorControl_WriteData();

#if !UPDATE_CAL_DATA_AFTER_ANY_SENSOR
        if ((calibrationDataUpdateRequired == 1) && (FC_IsDeviceCalibrationDone()))
        {
            /* Write updated data to EEPROM */
            EEPROM_WriteCalibrationData();

            /* Clear the flag */
            calibrationDataUpdateRequired = 0;
        }
#endif /* !UPDATE_CAL_DATA_AFTER_ANY_SENSOR */
    }
}

/*******************************************************************************
* Function Name: FlightController_ModeTweaks()
********************************************************************************
* Summary:
*  Main processing loop executes inside this function
*
* Parameters:
*  uint32_t currentTimeStamp - current time stamp of the microsecond timer
*
* Return:
*  None
*
*******************************************************************************/
static void FlightController_ModeTweaks(void)
{
#if GPS_INSTALLED
    /* Disable ROLL, PITCH, YAW and THROTTLE sticks during mission and RTH */
    if (GPS_Config.ignore_throttle == 1)
    {
        if ((globalFlags.GPS_MODE == GPS_MODE_NAV) || (globalFlags.GPS_MODE == GPS_MODE_RTH))
        {
            globalFlags.THROTTLE_IGNORED = 1;
        }
        else
        {
            globalFlags.THROTTLE_IGNORED = 0;
        }
    }
#endif /* GPS_INSTALLED */

#if MAG_INSTALLED
    if (abs(rcCommand[YAW]) <70 && (globalFlags.MAG_MODE))
    {
          int16_t dif = att.heading - magHold;
          if (dif <= - 180)
          {
              dif += 360;
          }

          if (dif >= + 180)
          {
              dif -= 360;
          }

          if (globalFlags.SMALL_ANGLES_25  || (globalFlags.GPS_MODE != GPS_MODE_NONE))
          {
              rcCommand[YAW] -= dif*FC_Config.PID[PIDMAG].P8>>5;
          }
    }
    else
    {
        magHold = att.heading;
    }
#endif /* MAG_INSTALLED */

#if BARO_INSTALLED && (!defined(SUPPRESS_BARO_ALTHOLD))
    /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
     * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
     */
    if (globalFlags.BARO_MODE)
    {
        static uint8_t isAltHoldChanged = 0;
        static int16_t AltHoldCorr = 0;

        if (globalFlags.LAND_IN_PROGRESS)
        {
            AltHoldCorr -= GPS_Config.land_speed;
            if (abs(AltHoldCorr) > 512)
            {
                AltHold += AltHoldCorr / 512;
                AltHoldCorr %= 512;
            }
        }

        if ((abs(rcCommand[THROTTLE] - initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) && (!globalFlags.THROTTLE_IGNORED))
        {
            /* Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) */
            AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;
            if (abs(AltHoldCorr) > 512)
            {
                AltHold += AltHoldCorr / 512;
                AltHoldCorr %= 512;
            }

            isAltHoldChanged = 1;
        }
        else if (isAltHoldChanged)
        {
            AltHold = alt.estAltitude;
            isAltHoldChanged = 0;
        }

        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
#endif /* BARO_INSTALLED */

#if defined(THROTTLE_ANGLE_CORRECTION)
    if (globalFlags.ANGLE_MODE || globalFlags.HORIZON_MODE)
    {
       rcCommand[THROTTLE]+= throttleAngleCorrection;
    }
#endif
}

/*******************************************************************************
* Function Name: FlightController_SystemArm()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_SystemArm(void)
{
    if ((calibration.GyroCycles == 0)
#if defined(ONLYARMWHENFLAT)
        && (globalFlags.ACCL_CALIBRATED)
#endif /* defined(ONLYARMWHENFLAT) */
        )
    {
        if(!globalFlags.ARMED && !globalFlags.BARO_MODE)
        {
            /* arm now! */
            globalFlags.ARMED = 1;
            headFreeModeHold = att.heading;
            magHold = att.heading;
         }
    }
    else if (!globalFlags.ARMED)
    {
        /* Indicate that the accelerometer or gyro is not calibrated yet */
        Alarms_BlinkLED(2,255,1);
    #if defined(BUZZER)
        Alarms_SetAlarmLevel(ALARM_EVENT_ACCELEROMETER, ALARM_LEVEL_1);
    #endif /* defined(BUZZER) */
    }
}

/*******************************************************************************
* Function Name: FlightController_SystemDisarm()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_SystemDisarm(void)
{
    if (globalFlags.ARMED)
    {
        globalFlags.ARMED = 0;
    }
}



extern uint16_t deltaT;
void testFlow(void)
{

    Debug_BufferAppendString("Debug Message \n");
    Debug_SendMessage(16);



#if 0
    static uint32_t tmpTime = micros();

    if (abs(micros() - tmpTime) < 10000)
        return;

    tmpTime = micros();
    /* Temporary - just read ADC data from the accelerometer */
    //Accelerometer_ReadRawADC();
    //Gyroscope_ReadRawADC();

    //Debug_BufferAppendString("Test Accelerometer Data Output \n");
    //Debug_SendMessage(100);

    /* Write the data to the host */
    CommSerial_SendUint8(0, 0xA5);
    CommSerial_SendUint8(0, 0x5A);
    CommSerial_SendUint8(0, 0x01);
    CommSerial_SendUint8(0, 0x0B);
    //CommSerial_SendUint16(0, uint16_t(imuData.accADC[0]));att.angle
    //CommSerial_SendUint16(0, uint16_t(imuData.accADC[1]));
    CommSerial_SendUint16(0, uint16_t(att.angle[0]));
    CommSerial_SendUint16(0, uint16_t(att.angle[1]));
    CommSerial_SendUint16(0, uint16_t(imuData.accADC[2]));
    CommSerial_SendUint16(0, uint16_t(imuData.gyroADC[0]));
    CommSerial_SendUint16(0, uint16_t(imuData.gyroADC[1]));
    CommSerial_SendUint16(0, uint16_t(imuData.gyroADC[2]));
    //CommSerial_WriteCRLF(0);
    //delay(10);

    /* Process Receiver channels data */
    RC_ProcessReceivedData();

    /* Send the data for Rc Data */
    CommSerial_SendUint8(0, 0xA5);
    CommSerial_SendUint8(0, 0x5A);
    CommSerial_SendUint8(0, 0x02);
    CommSerial_SendUint8(0, 0x10);
    CommSerial_SendUint16(0, uint16_t(rcData[THROTTLE]));
    CommSerial_SendUint16(0, uint16_t(rcData[ROLL]));
    CommSerial_SendUint16(0, uint16_t(rcData[PITCH]));
    CommSerial_SendUint16(0, uint16_t(rcData[YAW]));
    CommSerial_SendUint16(0, uint16_t(rcData[AUX1]));
    CommSerial_SendUint16(0, uint16_t(rcData[AUX2]));
    CommSerial_SendUint16(0, uint16_t(rcData[AUX3]));
    CommSerial_SendUint16(0, uint16_t(rcData[AUX4]));

    delay(5);
#endif


}

